#pragma once
#include "ros/ros.h"
#include "self_localization_transform/usv_self.h"
#include "sensor_msgs/NavSatFix.h"
#include "sensor_msgs/Imu.h"
#include "gnss_imu_align/gnss_imu_align.h"
#include <deque>
enum GNSSType{
    xt_usv = 0,
    normal = 1
};
class GNSS_IMU_Align_Wrapper
{
private:
    ros::Subscriber gnss_sub;
    ros::Subscriber imu_sub;
    ros::Publisher path_pub;
    ros::Publisher odom_pub;
    std::string gnss_topic;
    std::string imu_topic;
    std::string path_topic;
    std::string odom_topic;
    int gnss_type;
    std::deque<SlamCraft::GNSSDataType> gnss_buffer;
    std::deque<SlamCraft::IMU> imu_buffer;
    GNSS_IMU_Align gnss_imu_align;
public:

    GNSS_IMU_Align_Wrapper(/* args */);
    ~GNSS_IMU_Align_Wrapper();
    void xt_gnss_sub_cb(const self_localization_transform::usv_selfConstPtr &msg);
    void normal_gnss_sub_cb(const sensor_msgs::NavSatFixConstPtr &msg);
    void imu_sub_cb(const sensor_msgs::ImuConstPtr &msg);
    void run();
    bool sync_measure_group(MeasureGroupType&measure_group);
    void publish_path(Eigen::Vector3d t,double time_stamp);
    void publish_odometry( SlamCraft::ESKF::State_18 state_18,double time_stamp);
    void publish_odometry( Eigen::Vector3d position,double time_stamp);
};

